//==============================================================
//ME
//--------------------------------------------------
//Function description:
//  mex version of me 
//  motion estimation
//
//  parameter : 
//              cur_pic(UINT8 YUV[]),
//              pre_pic(UINT8 YUV[]),
//              pic_pola, (PIC_POLARITY: prog(0), top(1), bot(2)),
//              chrom_type, (CHROMA: 422, 444, 420, 422)
//              me_type(ME_TYPE),
//              ext_param_struct(STRUCT)
//
//  return    : out_result(STRUCT)
//
//------------------------------------
//Editor: GVIM7.2
//------------------------------------
//$Author$
//$Revision$
//$Data$
//------------------------------------
//Log
//$Log$
//==============================================================


#include <math.h>
#include "mex.h"
//#include "portability.h"
typedef unsigned char     UINT8;
typedef char              INT8;
typedef unsigned short    UINT16;
typedef short             INT16;
typedef int               INT32;
typedef unsigned int      UINT32;
typedef enum 
{
	FRM_PARITY_PROGRESS     = 0,
	FRM_PARITY_TOP          = 1,
	FRM_PARITY_BOTTOM       = 2
} T_FRM_PARITY;

	//type: 0: zero vector
	//      2: temporal pred,   3: update
	//      4: spatial pred,    5: update
	//      16: affine model
	//      18: full search
	//      20: low resolution prediction
	//      22: cross phase
#define PRED_TYPE_ZV        0 
#define PRED_TYPE_TMP       2
#define PRED_TYPE_TMP_UPD   3
#define PRED_TYPE_SPT       4
#define PRED_TYPE_SPT_UPD   5
#define PRED_TYPE_GM        16
#define PRED_TYPE_FS        18
#define PRED_TYPE_LR        20
#define PRED_TYPE_CP        22
typedef struct
{
   INT16    vector[2];
   UINT16	penalty;
   UINT8    type;
}  T_CAND_VEC;

typedef struct
{
	UINT8      pred_mode;      //0: original pred mode;  1: apl adaptive pred mode
	UINT8      pred_sad_factor;
	bool       same_temp_en;   //1: prediction vector have same temporal position, can use the history information to enable 2fld mode
	bool       cur_high_priority;  //cur me have high priority than prediction vector field
	UINT16     same_temp_penalty;
	bool       flipud_en;      //0: vertical flip
	UINT8      cuc_sad_diff_thr_k; //cover uncover sad difference threshold is 
	UINT16     cuc_sad_diff_thr;   //cuc_sad_diff_thr + cuc_sad_diff_thr_k * smaller_sad/16
	UINT32     cuc_sad_thr;        //sad_bigger > cuc_sad_thr
    UINT8      block_size[2];
    UINT16     block_number[2];
    bool       pred_en[8];
    bool       update_en[8];
    INT8       pred_pos[8][2];
    UINT16     pred_penalty[8];
    UINT16     update_penalty;
    UINT8      update_level[2];
    bool       zero_en;
    UINT16     zero_penalty;
    bool       afm_en;
    INT16      afm_param[6];
    UINT16     afm_penalty;
	bool       lrpred_en;    //pen = lrp_k_dtl * dtl + lrp_k_apl * apl_diff + lrp_penalty
	UINT8      lrp_k_dtl;
	UINT8      lrp_k_apl; 
	UINT16     lrp_penalty;
    UINT8      vec_precision[2];
    bool       y_only;
    INT16*     prev_vec;
	UINT32*    prev_sad;
	UINT8*     prev_apl;
	UINT16*    prev_dtl;
	INT16*     prev_lr_vec;
	UINT32*    prev_lr_sad;
	UINT8*     prev_lr_apl;
	UINT16*    prev_lr_dtl;
	UINT8      lr_block_ratio[2];
    UINT8      temp_phase;//128 totally, 0 @ previous, 128 @ current
	bool       meander_en;
	UINT8      overlap[2];// block = block_size + 2*overlap (overlap is one side)
	bool       fs_en;
	UINT16     fs_penalty;
	UINT16     fs_step[8];
	bool       aplap_en;
	UINT8      aplap_vdiff_factor;
	UINT8      aplap_mm_apl_diff_thr;
	UINT16     aplap_mm_vec_diff_thr;
	UINT16     mm_sad_diff_th;
	UINT16     mm_detail_best_th;
	UINT16     mm_detail_sec_th;
	UINT8      mm_detail_k_th;
	UINT8      mm_mode;
	UINT8      mm_vector_cons_th0;
	UINT8      mm_vector_cons_th1;
	UINT8      mm_vector_cons_th2;
	bool       mm_en;
	bool       mm_med_en;
	UINT16     flh_th;
	UINT8      crossphase_en;
	UINT16     cp_penalty;
} T_3DRS_PARAM;

//


#undef _INCLUDE_
//#define _DEBUG_

#ifdef 		    _DEBUG_
    #define 		_DEBUG_PRED_VECTOR
    #define 		_DEBUG_GET_CURRENT_BLOCK
    #define 		_DEBUG_GET_REF_BLOCK
    #define 		_DEBUG_CALC_BLK_ERR
    #define 		_DEBUG_UPDATE_MV
    #define 		_DEBUG_3DR_ME
	#define         _DEBUG_FLASH_
#else
    #define         mexPrintf   //
#endif

#define MAX_BLK_SIZE_X             64
#define MAX_BLK_SIZE_Y             64
//input parameters declare
//data path input
#define PREVIOUS_Y                  prhs[0]
#define PRE_POLARITY                prhs[1]
#define CURRENT_Y                   prhs[2]
#define CUR_POLARITY                prhs[3]
#define FRAME_FORMAT                prhs[4]
#define ME_PARAMETER                prhs[5]

//output parameters declare
#define ME_RESULT                   plhs[0]

static INT16 UPDATEi[][2]				= 	{{1,0},{-1,0},{0,1},{0,-1},{2,0},{-2,0},{0,2},{0,-2}};
static INT16 UPDATEf[][2]				= 	{{1,0},{-1,0},{0,1},{0,-1}};
static UINT16 FLD_ID                    =   0;

UINT8 ui8diff(UINT8 a, UINT8 b)
{
	return a>b?a-b:b-a;
}

void  get_update(const UINT8* pLevel, const UINT8* pPrecision, INT16* pUpdate)
{
	int iRandIndx;
    iRandIndx = 	(int)floor(8*rand()/(RAND_MAX+1));
    pUpdate[0] = UPDATEi[iRandIndx][0]*((pLevel[0]*pPrecision[0]));
    pUpdate[1] = UPDATEi[iRandIndx][1]*((pLevel[1]*pPrecision[1]));
    
    iRandIndx = (int)floor(4*rand()/(RAND_MAX+1));
    pUpdate[0] += UPDATEf[iRandIndx][0] ;
    pUpdate[1] += UPDATEf[iRandIndx][1] ;
    return;
}

bool get_raw_block(const UINT8* pY, UINT16 iYRowByte, UINT8 iYPxlByte, 
                   const UINT16* pPicSize,T_FRM_PARITY iPolarity,
                   const UINT8* pBlkSize, const INT16* iTlPos,
                   UINT8* pRawY)
{
    INT16 iXPos[MAX_BLK_SIZE_X];
	INT16 iYPos[MAX_BLK_SIZE_Y];
	INT16 iYPosUp[MAX_BLK_SIZE_Y];
	INT16 iYPosDn[MAX_BLK_SIZE_Y];
	int   iAdr, iAdrUp, iAdrDn;
	int   i,j;
	for(i=0; i<= pBlkSize[0]; i++)
	{
		iXPos[i] = iTlPos[0] + i;
		if (iXPos[i] < 0)
			iXPos[i] = 0;
		if (iXPos[i] >= pPicSize[0])
			iXPos[i] = pPicSize[0]-1;
	}
	for(i=0; i<= pBlkSize[1]; i++)
	{
		iYPos[i] = iTlPos[1] + i;
		if(iPolarity == 2)//(T_FRM_PARITY::FRM_PARITY_BOTTOM)
			iYPos[i] = iYPos[i] -1;
		if (iYPos[i] < 0)
			iYPos[i] = 0;
		if(iPolarity == 0)//(T_FRM_PARITY::FRM_PARITY_PROGRESS)
		{
			if (iYPos[i] >= pPicSize[1])
				iYPos[i] = pPicSize[1]-1;
		} else
		{
			if (iYPos[i] > (pPicSize[1]-1)*2)
				iYPos[i] = (pPicSize[1]-1)*2;
		}
	}

	if(iPolarity != 0) //T_FRM_PARITY::FRM_PARITY_PROGRESS)
	{
		for(i=0; i<= pBlkSize[1]; i++)
		{
			iYPosUp[i] = iYPos[i] >> 1;
			iYPosDn[i] = (iYPos[i]+1) >> 1;
		}
	}

	for(i=0; i<= pBlkSize[0]; i++)
		for (j=0; j<= pBlkSize[1]; j++)
		{
			if(iPolarity == 0)//T_FRM_PARITY::FRM_PARITY_PROGRESS)
			{
				iAdr = iYPos[j]*iYRowByte +iXPos[i]*iYPxlByte;
				pRawY[i+j*(pBlkSize[0]+1)] = pY[iAdr];
			}
			else{
				iAdrUp = iYPosUp[j]*iYRowByte +iXPos[i]*iYPxlByte;
                iAdrDn = iYPosDn[j]*iYRowByte +iXPos[i]*iYPxlByte;
				pRawY[i+j*(pBlkSize[0]+1)] = (UINT8) ((pY[iAdrUp]/2.0) + (pY[iAdrDn]/2.0));
			}

		}

    return true;
}

bool bl_interp_block(const UINT8* pRawY, const UINT8* pBlkSize, const UINT8* pPhase, const UINT8* pVecPrecision,
					 UINT8* pData)
{
	UINT16 iX, iY;
	float  fDat;
	UINT16 iKTl, iKTr, iKBl, iKBr;
	iKTl = (pVecPrecision[0]-pPhase[0])*(pVecPrecision[1]-pPhase[1]); 
    iKTr = pPhase[0]*(pVecPrecision[1]-pPhase[1]);
    iKBl = (pVecPrecision[0]-pPhase[0])*pPhase[1];
    iKBr = pPhase[0] * pPhase[1];
	for(iX = 0; iX < pBlkSize[0]; iX=iX+1)
		for(iY=0; iY<pBlkSize[1]; iY=iY+1)
		{
			fDat = (float)(pRawY[iX+iY*(pBlkSize[0]+1)] * iKTl +
				          pRawY[iX+1+iY*(pBlkSize[0]+1)] * iKTr +
                          pRawY[iX+(iY+1)*(pBlkSize[0]+1)] * iKBl +
                          pRawY[iX+1+(iY+1)*(pBlkSize[0]+1)] * iKBr) / (float)(pVecPrecision[0]*pVecPrecision[1]);
			pData[iX+iY*pBlkSize[0]] = (fDat > 255) ? 255 : (UINT8) fDat;
		}
    return true;
}
                  
    
void   get_block(const UINT8* pY, UINT16 iYRowByte, UINT8 iYPxlByte,  
                 const UINT16* pPicSize, T_FRM_PARITY iPolarity, const INT16* pPos, 
                 const T_CAND_VEC* pCandVec, const UINT8* pVecPrecision, UINT8 iTempPhase, 
                 const UINT8* pBlkSize, bool bPrev, 
                 UINT8* pBlkData)
{
    INT16 iTlPos[2];
    UINT8 iPhase[2];
    //UINT8 iPxlIdx[2];
    //UINT8 iStep;
    UINT8 pRawY[(MAX_BLK_SIZE_X +1)* (MAX_BLK_SIZE_Y+1)];
    //UINT8* pCurData;

	//get_tl_pos(pPos, iTempPhase, pCandVec, pVecPrecision, bPrev, iTlPos, iPhase);
    //D
    INT16    iVecInt[2];
  
    double   dVec[2];
    double   dAlpha;
    //vector retimer
    dAlpha = bPrev?(-iTempPhase/128.0):((128-iTempPhase)/128.0);
    dVec[0] = pCandVec->vector[0]*dAlpha;
    dVec[1] = pCandVec->vector[1]*dAlpha;                                         
    iVecInt[0] = (INT16)floor(dVec[0]/pVecPrecision[0]);
    iVecInt[1] = (INT16)floor(dVec[1]/pVecPrecision[1]);
    iPhase[0] = (UINT8)(dVec[0]-iVecInt[0]*pVecPrecision[0]);
    iPhase[1] = (UINT8)(dVec[1]-iVecInt[1]*pVecPrecision[1]);
    iTlPos[0] = pPos[0]+iVecInt[0];
    iTlPos[1] = pPos[1]+iVecInt[1];

    //iStep = bYOnly? 1:3; 
    get_raw_block(pY, iYRowByte, iYPxlByte, 
                  pPicSize, iPolarity,
                  pBlkSize, iTlPos,
                  pRawY);
                  
    bl_interp_block(pRawY, pBlkSize, iPhase, pVecPrecision,
		          pBlkData);
    return;
}


UINT32 get_sad(UINT8* pPreBlkData, UINT8* pCurBlkData, UINT16 iPxlNumInBlk, bool bYOnly)
{
    UINT32 result;
    UINT16  iIdx, iNum;
    result = 0;
    iNum = bYOnly? iPxlNumInBlk : 3*iPxlNumInBlk;
    for(iIdx=0; iIdx<iNum; iIdx++)
        result += ((pPreBlkData[iIdx]>pCurBlkData[iIdx])? (pPreBlkData[iIdx]-pCurBlkData[iIdx]):
                                                         (pCurBlkData[iIdx]-pPreBlkData[iIdx]));
    return result;
}

bool flashing_detect(UINT8* pPreAPL,UINT16 iBlkIdx,UINT8 iCurBlkAPL ,UINT16 iFlhThr)
{
   bool bFlh;
   UINT16 iAplDiff;
   iAplDiff = pPreAPL[iBlkIdx]>iCurBlkAPL?(pPreAPL[iBlkIdx]-iCurBlkAPL):(iCurBlkAPL-pPreAPL[iBlkIdx]);
   bFlh = iAplDiff >= iFlhThr;
   return bFlh;
}

bool    apl_adaptive_penalty(UINT8 iCurBlkAPL, const T_3DRS_PARAM* p3DRSParam,  INT16* pBlkPos, T_CAND_VEC* pCandVec,UINT8 iCandNum)
{
	INT16  iNeighborBlkPos[2];
	INT8   iIdx[2];
	INT16  iBlkIdx;
	INT16  iVector[25][2], iMaxSum[2], iMinSum[2], iMaxMeanVec[2], iMinMeanVec[2], iMMVecDiff[2], iWishVec[2], iWishVecDiff[2];
	UINT8  iAPL[25], iMaxAPL, iMinAPL;
	UINT8  iItemNum;
	INT16  iTotalBlkNum;
	UINT16 iGroupEdge, iMinNum, iMaxNum, iGrpIdx, iCandIdx;
	/* get neighbor vector */
	iTotalBlkNum = p3DRSParam->block_number[0]*p3DRSParam->block_number[1];
	iItemNum = 0;
	iMaxAPL = 0; iMinAPL=255;
	for(iIdx[1] = -2; iIdx[1]<=2; iIdx[1]++)
	{
		for(iIdx[0] = -2; iIdx[0]<=2; iIdx[0]++)
		{
			iNeighborBlkPos[0] = pBlkPos[0] + iIdx[0];
			iNeighborBlkPos[1] = pBlkPos[1] + iIdx[1];
			if((iNeighborBlkPos[0]<0)||(iNeighborBlkPos[0]>=p3DRSParam->block_number[0])||
			   (iNeighborBlkPos[1]<0)||(iNeighborBlkPos[1]>=p3DRSParam->block_number[1]))
				continue;
			else
			{
                iBlkIdx              = iNeighborBlkPos[1]*p3DRSParam->block_number[0]+iNeighborBlkPos[0];
				iAPL[iItemNum]       = p3DRSParam->prev_apl[iBlkIdx];
				iVector[iItemNum][0] = p3DRSParam->prev_vec[iBlkIdx];
				iVector[iItemNum][1] = p3DRSParam->prev_vec[iBlkIdx+iTotalBlkNum];
				if(iAPL[iItemNum]>iMaxAPL)
					iMaxAPL = iAPL[iItemNum];
				if(iAPL[iItemNum]<iMinAPL)
					iMinAPL = iAPL[iItemNum];
				iItemNum++;
			}
		}
	}
	/* cluster vector/apl */
	if((iMaxAPL - iMinAPL) < p3DRSParam->aplap_mm_apl_diff_thr)
		return true;
	else
	{
		iMinNum=0; iMaxNum=0;
		iMaxSum[0]=0;iMaxSum[1]=0; 
		iMinSum[0]=0;iMinSum[1]=0;
		iGroupEdge = ((UINT16)iMaxAPL + iMinAPL)/2;
		for(iGrpIdx = 0; iGrpIdx<iItemNum; iGrpIdx++)
		{
			if(iAPL[iGrpIdx]<iGroupEdge)
			{
				iMinNum++;
				iMinSum[0] += iVector[iGrpIdx][0];
				iMinSum[1] += iVector[iGrpIdx][1];
			} else
			{
				iMaxNum++;
				iMaxSum[0] += iVector[iGrpIdx][0];
				iMaxSum[1] += iVector[iGrpIdx][1];
			}
		}
		if(iMinNum==0 || iMaxNum==0)
			return true;
		else
		{
			iMaxMeanVec[0] = iMaxSum[0] / iMaxNum;
			iMaxMeanVec[1] = iMaxSum[1] / iMaxNum;
			iMinMeanVec[0] = iMinSum[0] / iMinNum;
			iMinMeanVec[1] = iMinSum[1] / iMinNum;
		}
		iMMVecDiff[0] = (iMaxMeanVec[0] > iMinMeanVec[0]) ? (iMaxMeanVec[0] - iMinMeanVec[0]):(iMinMeanVec[0] - iMaxMeanVec[0]);
		iMMVecDiff[1] = (iMaxMeanVec[1] > iMinMeanVec[1]) ? (iMaxMeanVec[1] - iMinMeanVec[1]):(iMinMeanVec[1] - iMaxMeanVec[1]);
		if((iMMVecDiff[0]+iMMVecDiff[1])< p3DRSParam->aplap_mm_vec_diff_thr)
			return true;
	}

	/* update cand vector penalty */
	if(iCurBlkAPL > iGroupEdge)
	{
		iWishVec[0] = iMaxMeanVec[0];
		iWishVec[1] = iMaxMeanVec[1];
	} else
	{
		iWishVec[0] = iMinMeanVec[0];
		iWishVec[1] = iMinMeanVec[1];
	}

	for(iCandIdx=0; iCandIdx<iCandNum; iCandIdx++)
	{
		if(pCandVec[iCandIdx].type == PRED_TYPE_ZV ||
           pCandVec[iCandIdx].type == PRED_TYPE_GM ||
           pCandVec[iCandIdx].type == PRED_TYPE_FS ||
           pCandVec[iCandIdx].type == PRED_TYPE_LR ||
           pCandVec[iCandIdx].type == PRED_TYPE_CP)
		   continue;
		iWishVecDiff[0] = (pCandVec[iCandIdx].vector[0]>iWishVec[0])? (pCandVec[iCandIdx].vector[0] - iWishVec[0]):(iWishVec[0] - pCandVec[iCandIdx].vector[0] );
		iWishVecDiff[1] = (pCandVec[iCandIdx].vector[1]>iWishVec[1])? (pCandVec[iCandIdx].vector[1] - iWishVec[1]):(iWishVec[1] - pCandVec[iCandIdx].vector[1] );
		pCandVec[iCandIdx].penalty += (iWishVecDiff[0]+iWishVecDiff[1])*p3DRSParam->aplap_vdiff_factor;
	}

	return true;
}



void   pred_vector_apl_adapt(UINT8 iCurBlkAPL, 
	                             const T_3DRS_PARAM* p3DRSParam, T_CAND_VEC* pCandVec, 
	                             INT16* pCurVecFld, UINT8* pCurAPL, INT16* pBlkPos, bool bRewind ,INT16 iMedVec[2])
{
	UINT16 iBlkNumX, iBlkNumY;
	UINT16 iLRBlkNumX, iLRBlkNumY;
	UINT16 iLRBlkPos[2];
	UINT8  iBlkSize[2];
	UINT8  iPredIdx,iCandIdx;
	INT16  iPredBlkPos[2];
	INT16  iUpdate[2];
	INT16* pVecFld; 
	UINT8* pAPLFld;
	UINT8  iPredAPL;
	T_CAND_VEC tZeroVec;
	INT16  iPxlPos[2];
	//UINT8  iCurBlk[4096], iRefBlk[4096];
	UINT32  iSAD;
	UINT16  iPxlNumInBlk;
	bool    bSpatialPred;
	
	UINT8  iFsPtnIdx;
	INT16  iFSVecX, iFSVecY;
	INT8   iX,iY;
	INT16  iVecX55Sum,iVecY55Sum;

	iBlkNumX  =  p3DRSParam->block_number[0];
	iBlkNumY  =  p3DRSParam->block_number[1];
	iLRBlkNumX=  (UINT16)ceil(p3DRSParam->block_number[0]*1.0/(p3DRSParam->lr_block_ratio[0]));
    iLRBlkNumY=  (UINT16)ceil(p3DRSParam->block_number[1]*1.0/(p3DRSParam->lr_block_ratio[1]));

	iBlkSize[0] = p3DRSParam->block_size[0];
	iBlkSize[1] = p3DRSParam->block_size[1];
	iPxlNumInBlk = iBlkSize[0]*iBlkSize[1];
	iCandIdx = 0;
	tZeroVec.vector[0] = 0;
	tZeroVec.vector[1] = 0;

	iPxlPos[0] = pBlkPos[0] * iBlkSize[0];
	iPxlPos[1] = pBlkPos[1] * iBlkSize[1];
	iVecX55Sum = 0;
	iVecY55Sum = 0;


	for(iY=-2;iY<=2;iY++)
	   {
        for(iX=-2;iX<=2;iX++)
        	{
			iPredBlkPos[0] = pBlkPos[0] - iX;
			if(iPredBlkPos[0]<0) iPredBlkPos[0] = 0;
			else if(iPredBlkPos[0]>=iBlkNumX) iPredBlkPos[0] = iBlkNumX-1;
			iPredBlkPos[1] = pBlkPos[1] - iY;
			if(iPredBlkPos[1]<0) iPredBlkPos[1] = 0;
			else if(iPredBlkPos[1]>=iBlkNumY) iPredBlkPos[1] = iBlkNumY-1;

			bSpatialPred = (iY < 0)&&(iPredBlkPos[1]!=0);
									  //(((p3DRSParam->pred_pos[iPredIdx][1]<0)&&(iPredBlkPos[1]!=0)) || 
									  //(p3DRSParam->pred_pos[iPredIdx][1]==0&&p3DRSParam->pred_pos[iPredIdx][0]<0&&(iPredBlkPos[0]!=0)))
			pVecFld = 
				(bSpatialPred? pCurVecFld:(p3DRSParam->prev_vec));
            iVecX55Sum += pVecFld[iPredBlkPos[1]*iBlkNumX+iPredBlkPos[0]];
			iVecY55Sum += pVecFld[iPredBlkPos[1]*iBlkNumX+iPredBlkPos[0]+iBlkNumX*iBlkNumY];

			
		    }

	   }
    iMedVec[0]= iVecX55Sum/25;
	iMedVec[1]= iVecY55Sum/25;
	
	//get_block(pYUV, pPicSize, iPolarity,iPxlPos, &tZeroVec, p3DRSParam->vec_precision, 0, pBlkSize, true, true, iCurBlk);
	for(iPredIdx=0; iPredIdx<8; iPredIdx++)
	{
		if(p3DRSParam->pred_en[iPredIdx])
		{
			pCandVec[iCandIdx].penalty = p3DRSParam->pred_penalty[iPredIdx];
			iPredBlkPos[0] = bRewind? (pBlkPos[0] - p3DRSParam->pred_pos[iPredIdx][0]):
			(pBlkPos[0] + p3DRSParam->pred_pos[iPredIdx][0]) ; 
			if(iPredBlkPos[0]<0) iPredBlkPos[0] = 0;
			else if(iPredBlkPos[0]>=iBlkNumX) iPredBlkPos[0] = iBlkNumX-1;
			iPredBlkPos[1] = p3DRSParam->flipud_en ? (pBlkPos[1] - p3DRSParam->pred_pos[iPredIdx][1]):
			(pBlkPos[1] + p3DRSParam->pred_pos[iPredIdx][1]);
			if(iPredBlkPos[1]<0) iPredBlkPos[1] = 0;
			else if(iPredBlkPos[1]>=iBlkNumY) iPredBlkPos[1] = iBlkNumY-1;
			iPxlPos[0] = iPredBlkPos[0] * iBlkSize[0];
			iPxlPos[1] = iPredBlkPos[1] * iBlkSize[1];

			bSpatialPred = (p3DRSParam->pred_pos[iPredIdx][1]<0)&&(iPredBlkPos[1]!=0);
				          //(((p3DRSParam->pred_pos[iPredIdx][1]<0)&&(iPredBlkPos[1]!=0)) || 
				          //(p3DRSParam->pred_pos[iPredIdx][1]==0&&p3DRSParam->pred_pos[iPredIdx][0]<0&&(iPredBlkPos[0]!=0)))
            pAPLFld = (bSpatialPred? pCurAPL:(p3DRSParam->prev_apl));
			iPredAPL = pAPLFld[iPredBlkPos[1]*iBlkNumX+iPredBlkPos[0]];
			iSAD = ((iPredAPL>iCurBlkAPL)?(iPredAPL-iCurBlkAPL):(iCurBlkAPL-iPredAPL)) * p3DRSParam->pred_sad_factor;
			if(iSAD > 0xf0000000)
				iSAD = 0xf0000000;

			pVecFld = 
				(bSpatialPred? pCurVecFld:(p3DRSParam->prev_vec));
			pCandVec[iCandIdx].vector[0] = pVecFld[iPredBlkPos[1]*iBlkNumX+iPredBlkPos[0]];
			pCandVec[iCandIdx].vector[1] = pVecFld[iPredBlkPos[1]*iBlkNumX+iPredBlkPos[0]+iBlkNumX*iBlkNumY];
			pCandVec[iCandIdx].type      = bSpatialPred? PRED_TYPE_SPT : PRED_TYPE_TMP;
			if(p3DRSParam->pred_mode == 1)
				pCandVec->penalty           += iSAD;
			iCandIdx++;
			
			if(p3DRSParam->update_en[iPredIdx])
			{
				get_update(p3DRSParam->update_level, p3DRSParam->vec_precision, iUpdate);
				pCandVec[iCandIdx].vector[0] = pCandVec[iCandIdx-1].vector[0] + iUpdate[0];
				pCandVec[iCandIdx].vector[1] = pCandVec[iCandIdx-1].vector[1] + iUpdate[1];
				pCandVec[iCandIdx].penalty   = pCandVec[iCandIdx-1].penalty + p3DRSParam->update_penalty;
                pCandVec[iCandIdx].type      = pCandVec[iCandIdx-1].type + 1;
				iCandIdx++;
			}
		}
	}
	if(p3DRSParam->zero_en)
	{
		pCandVec[iCandIdx].vector[0] = 0;
		pCandVec[iCandIdx].vector[1] = 0;
		pCandVec[iCandIdx].penalty   = p3DRSParam->zero_penalty;
		pCandVec[iCandIdx].type      = PRED_TYPE_ZV;
		iCandIdx++;
	}

	if(p3DRSParam->afm_en)
	{
		pCandVec[iCandIdx].vector[0] = p3DRSParam->afm_param[0];
		pCandVec[iCandIdx].vector[1] = p3DRSParam->afm_param[1];
		pCandVec[iCandIdx].penalty   = p3DRSParam->afm_penalty;
		pCandVec[iCandIdx].type      = PRED_TYPE_GM;
		iCandIdx++;
	}

	if(p3DRSParam->fs_en)
	{
		iFsPtnIdx = (pBlkPos[0]+pBlkPos[1]+FLD_ID)%8;
        iFSVecX = p3DRSParam->fs_step[iFsPtnIdx];
		iFSVecY = iFSVecX/2;

		pCandVec[iCandIdx].vector[0] = -iFSVecX;
		pCandVec[iCandIdx].vector[1] = -iFSVecY;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		pCandVec[iCandIdx].type      = PRED_TYPE_FS;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = 0;
		pCandVec[iCandIdx].vector[1] = -iFSVecY;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		pCandVec[iCandIdx].type      = PRED_TYPE_FS;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = iFSVecX;
		pCandVec[iCandIdx].vector[1] = -iFSVecY;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		pCandVec[iCandIdx].type      = PRED_TYPE_FS;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = -iFSVecX;
		pCandVec[iCandIdx].vector[1] = 0;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		pCandVec[iCandIdx].type      = PRED_TYPE_FS;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = iFSVecX;
		pCandVec[iCandIdx].vector[1] = 0;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		pCandVec[iCandIdx].type      = PRED_TYPE_FS;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = -iFSVecX;
		pCandVec[iCandIdx].vector[1] = iFSVecY;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		pCandVec[iCandIdx].type      = PRED_TYPE_FS;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = 0;
		pCandVec[iCandIdx].vector[1] = iFSVecY;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		pCandVec[iCandIdx].type      = PRED_TYPE_FS;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = iFSVecX;
		pCandVec[iCandIdx].vector[1] = iFSVecY;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		pCandVec[iCandIdx].type      = PRED_TYPE_FS;
		iCandIdx++;

	}

	if(p3DRSParam->lrpred_en)
	{
		iLRBlkPos[0] = (UINT16)ceil((pBlkPos[0]*1.0/p3DRSParam->lr_block_ratio[0])-0.5);
		iLRBlkPos[1] = (UINT16)ceil((pBlkPos[1]*1.0/p3DRSParam->lr_block_ratio[1])-0.5);
		pCandVec[iCandIdx].vector[0] = p3DRSParam->prev_lr_vec[iLRBlkPos[0] + iLRBlkNumX*iLRBlkPos[1]] * p3DRSParam->lr_block_ratio[0];
		pCandVec[iCandIdx].vector[1] = p3DRSParam->prev_lr_vec[iLRBlkPos[0] + iLRBlkNumX*iLRBlkPos[1] + iLRBlkNumX*iLRBlkNumY] * p3DRSParam->lr_block_ratio[1];
		pCandVec[iCandIdx].penalty   = p3DRSParam->lrp_penalty + 
			                           p3DRSParam->lrp_k_apl * ui8diff(p3DRSParam->prev_lr_apl[iLRBlkPos[0] + iLRBlkNumX*iLRBlkPos[1]], iCurBlkAPL) +
									   p3DRSParam->lrp_k_dtl * p3DRSParam->prev_lr_dtl[iLRBlkPos[0] + iLRBlkNumX*iLRBlkPos[1]];		
		pCandVec[iCandIdx].type = PRED_TYPE_LR;
		iCandIdx++;
	}

    if(p3DRSParam->crossphase_en)
    {
       pCandVec[iCandIdx].vector[0] = p3DRSParam->prev_vec[pBlkPos[0] + pBlkPos[1]*iBlkNumX];
	   pCandVec[iCandIdx].vector[1] = p3DRSParam->prev_vec[pBlkPos[0] + pBlkPos[1]*iBlkNumX + iBlkNumX * iBlkNumY];
	   pCandVec[iCandIdx].penalty   = p3DRSParam->cp_penalty;
	   pCandVec[iCandIdx].type = PRED_TYPE_CP;
	   iCandIdx++;
	}

	if(p3DRSParam->aplap_en)
	    apl_adaptive_penalty(iCurBlkAPL, p3DRSParam, pBlkPos, pCandVec, iCandIdx);
	return;
}


int     get_cand_num(const T_3DRS_PARAM* p3DRSParam)
{
    int iNum;
    int iIdx;
    iNum = 0;
    for(iIdx=0; iIdx<8; iIdx++)
    {
        if(p3DRSParam->pred_en[iIdx])
        {
            iNum++;
            if(p3DRSParam->update_en[iIdx])
                iNum++;
        }
    }
    if(p3DRSParam->zero_en)
        iNum++;
    if(p3DRSParam->afm_en)
        iNum++;
	if(p3DRSParam->fs_en)
		iNum+=8;
	if(p3DRSParam->lrpred_en)
		iNum++;
	if(p3DRSParam->crossphase_en)
		iNum++;
    return iNum;
}

void   pred_vector(const T_3DRS_PARAM* p3DRSParam, T_CAND_VEC* pCandVec, INT16* pCurVecFld, INT16* pBlkPos, bool bRewind)
{
    UINT16 iBlkNumX, iBlkNumY;
    UINT8  iPredIdx,iCandIdx;
    INT16  iPredBlkPos[2];
    INT16  iUpdate[2];
	INT16* pVecFld; 
	UINT8  iFsPtnIdx;
	INT16  iFSVecX, iFSVecY;
	bool   bSpatialPred;
    iBlkNumX  =  p3DRSParam->block_number[0];
    iBlkNumY  =  p3DRSParam->block_number[1];
    iCandIdx = 0;
    for(iPredIdx=0; iPredIdx<8; iPredIdx++)
    {
        if(p3DRSParam->pred_en[iPredIdx])
        {
            pCandVec[iCandIdx].penalty = p3DRSParam->pred_penalty[iPredIdx];
			iPredBlkPos[0] = bRewind? (pBlkPos[0] - p3DRSParam->pred_pos[iPredIdx][0]):
			                          (pBlkPos[0] + p3DRSParam->pred_pos[iPredIdx][0]) ; 
            if(iPredBlkPos[0]<0) iPredBlkPos[0] = 0;
            else if(iPredBlkPos[0]>=iBlkNumX) iPredBlkPos[0] = iBlkNumX-1;
			iPredBlkPos[1] = p3DRSParam->flipud_en ? (pBlkPos[1] - p3DRSParam->pred_pos[iPredIdx][1]):
			                                         (pBlkPos[1] + p3DRSParam->pred_pos[iPredIdx][1]);
            if(iPredBlkPos[1]<0) iPredBlkPos[1] = 0;
            else if(iPredBlkPos[1]>=iBlkNumY) iPredBlkPos[1] = iBlkNumY-1;

			bSpatialPred = (p3DRSParam->pred_pos[iPredIdx][1]<0)&&(iPredBlkPos[1]!=0);
			pVecFld = bSpatialPred ? pCurVecFld:(p3DRSParam->prev_vec);
				//((((p3DRSParam->pred_pos[iPredIdx][1]<0)&&(iPredBlkPos[1]!=0)) || 
				//(p3DRSParam->pred_pos[iPredIdx][1]==0&&p3DRSParam->pred_pos[iPredIdx][0]<0&&(iPredBlkPos[0]!=0)))? pCurVecFld:(p3DRSParam->prev_vec));

            pCandVec[iCandIdx].vector[0] = pVecFld[iPredBlkPos[1]*iBlkNumX+iPredBlkPos[0]];
            pCandVec[iCandIdx].vector[1] = pVecFld[iPredBlkPos[1]*iBlkNumX+iPredBlkPos[0]+iBlkNumX*iBlkNumY];
            pCandVec[iCandIdx].type      = 4;
            //mexPrintf("#%d (%d,%d);", iCandIdx, pCandVec[iCandIdx].vector[0], pCandVec[iCandIdx].vector[1]);
            //mexPrintf("@(%d%d) vec(%d, %d);",iPredBlkPos[0], iPredBlkPos[1], p3DRSParam->prev_vec[2*(iPredBlkPos[1]*iBlkNumX+iPredBlkPos[0])], p3DRSParam->prev_vec[2*(iPredBlkPos[1]*iBlkNumX+iPredBlkPos[0])+1] );
            //mexPrintf("@(%d%d) vec(%d,%d);", pBlkPos[0], pBlkPos[1], p3DRSParam->prev_vec[2*(pBlkPos[0]+pBlkPos[1]*iBlkNumX)], p3DRSParam->prev_vec[2*(pBlkPos[0]+pBlkPos[1]*iBlkNumX)+1]);
            iCandIdx++;
            if(p3DRSParam->update_en[iPredIdx])
            {
                get_update(p3DRSParam->update_level, p3DRSParam->vec_precision, iUpdate);
                pCandVec[iCandIdx].vector[0] = pCandVec[iCandIdx-1].vector[0] + iUpdate[0];
                pCandVec[iCandIdx].vector[1] = pCandVec[iCandIdx-1].vector[1] + iUpdate[1];
                pCandVec[iCandIdx].penalty   = pCandVec[iCandIdx-1].penalty + p3DRSParam->update_penalty;

                iCandIdx++;
            }
        }
    }
	if(p3DRSParam->zero_en)
	{
		pCandVec[iCandIdx].vector[0] = 0;
		pCandVec[iCandIdx].vector[1] = 0;
		pCandVec[iCandIdx].penalty   = p3DRSParam->zero_penalty;
		iCandIdx++;
	}
	if(p3DRSParam->afm_en)
	{
		pCandVec[iCandIdx].vector[0] = p3DRSParam->afm_param[0];
		pCandVec[iCandIdx].vector[1] = p3DRSParam->afm_param[1];
		pCandVec[iCandIdx].penalty   = p3DRSParam->afm_penalty;
		iCandIdx++;
	}
	if(p3DRSParam->fs_en)
	{
		iFsPtnIdx = (pBlkPos[0]+pBlkPos[1]+FLD_ID)%8;
        iFSVecX = p3DRSParam->fs_step[iFsPtnIdx];
		iFSVecY = iFSVecX/2;

		pCandVec[iCandIdx].vector[0] = -iFSVecX;
		pCandVec[iCandIdx].vector[1] = -iFSVecY;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = 0;
		pCandVec[iCandIdx].vector[1] = -iFSVecY;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = iFSVecX;
		pCandVec[iCandIdx].vector[1] = -iFSVecY;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = -iFSVecX;
		pCandVec[iCandIdx].vector[1] = 0;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = iFSVecX;
		pCandVec[iCandIdx].vector[1] = 0;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = -iFSVecX;
		pCandVec[iCandIdx].vector[1] = iFSVecY;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = 0;
		pCandVec[iCandIdx].vector[1] = iFSVecY;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		iCandIdx++;

		pCandVec[iCandIdx].vector[0] = iFSVecX;
		pCandVec[iCandIdx].vector[1] = iFSVecY;
		pCandVec[iCandIdx].penalty   = p3DRSParam->fs_penalty;
		iCandIdx++;

	}
    return;
}

bool verify_common_parameters (UINT16 iCurSize[2], T_FRM_PARITY  tCurPolarity,
                               UINT16 iPreSize[2], T_FRM_PARITY  tPrePolarity)
{
    return true;
}


UINT8 calc_apl(const UINT8* pBlk, const UINT8* pBlkSize)
{
    int iIdx;
	UINT16 iSum;
	UINT16 iTotalPxl;
	double dAPL;
	UINT8  iAPL;
	iSum = 0;
	iTotalPxl = pBlkSize[0]*pBlkSize[1];
	for (iIdx=0; iIdx<iTotalPxl; iIdx++)
		iSum += pBlk[iIdx];
	dAPL = iSum * 1.0 / iTotalPxl;
	iAPL = dAPL > 255 ? 255:(UINT8)dAPL;
	return iAPL;
}

UINT16 calc_detail(const UINT8* pBlk, const UINT8* pBlkSize)
{
	int iIdx[2];
	UINT16 iSum;
	iSum = 0;
	/*y diff*/
	for(iIdx[0]=0; iIdx[0]<pBlkSize[0]; iIdx[0]++)
		for(iIdx[1]=0; iIdx[1]<pBlkSize[1]-1; iIdx[1]++)
			iSum += (pBlk[iIdx[0]+iIdx[1]*pBlkSize[0]] > pBlk[iIdx[0]+(iIdx[1]+1)*pBlkSize[0]]) ? (pBlk[iIdx[0]+iIdx[1]*pBlkSize[0]] - pBlk[iIdx[0]+(iIdx[1]+1)*pBlkSize[0]]):
			                                                                                     (pBlk[iIdx[0]+(iIdx[1]+1)*pBlkSize[0]]-pBlk[iIdx[0]+iIdx[1]*pBlkSize[0]]);

	/*x diff*/
	for(iIdx[1]=0; iIdx[1]<pBlkSize[1]; iIdx[1]++)
		for(iIdx[0]=0; iIdx[0]<pBlkSize[0]-1; iIdx[0]++)
			iSum += (pBlk[iIdx[0]+iIdx[1]*pBlkSize[0]] > pBlk[iIdx[0]+1+iIdx[1]*pBlkSize[0]]) ? (pBlk[iIdx[0]+iIdx[1]*pBlkSize[0]] - pBlk[iIdx[0]+1+iIdx[1]*pBlkSize[0]]):
			                                                                                     (pBlk[iIdx[0]+1+iIdx[1]*pBlkSize[0]]-pBlk[iIdx[0]+iIdx[1]*pBlkSize[0]]);

	return (UINT16)(iSum/((pBlkSize[0]*(pBlkSize[1]-1))+((pBlkSize[0]-1)*pBlkSize[1])));
}

bool multi_min(const T_CAND_VEC* pBestVec, const T_CAND_VEC* pSecVec, UINT32 iMinSAD, UINT32 iSecSAD, UINT16 iBestDetail, UINT16 iSecDetail,INT16 iMedVec[2],
	             UINT16 pMm_sad_diff_th, UINT16 pMm_detail_best_th, UINT16 pMm_detail_sec_th, UINT8 pMm_detail_k_th, UINT8 pMm_mode ,
	             UINT8 pMm_vec_cons_th0, UINT8 pMm_vec_cons_th1, UINT8 pMm_vec_cons_th2, bool pMm_med_en )
{
   UINT16 sad_diff_th;
   UINT16 vec_diff_x;
   UINT16 vec_diff_y;
   UINT16 vec_diff;
   bool   small_sad_diff,big_detail_diff;
   bool   select_sec;
   vec_diff_x = (pBestVec->vector[0] > pSecVec->vector[0])?(pBestVec->vector[0]- pSecVec->vector[0]):(pSecVec->vector[0]-pBestVec->vector[0]);
   vec_diff_y = (pBestVec->vector[1] > pSecVec->vector[1])?(pBestVec->vector[1]- pSecVec->vector[1]):(pSecVec->vector[1]-pBestVec->vector[1]);
   vec_diff   =  vec_diff_x + vec_diff_y;
   sad_diff_th = vec_diff < pMm_vec_cons_th0? pMm_sad_diff_th : 
   	             vec_diff < pMm_vec_cons_th1? pMm_sad_diff_th * 2:
				 vec_diff < pMm_vec_cons_th2? pMm_sad_diff_th * 3:
				 pMm_sad_diff_th * 4;	

   small_sad_diff =( iSecSAD - iMinSAD ) < sad_diff_th;
   big_detail_diff = (iBestDetail < pMm_detail_best_th )&
   	                 (iSecDetail  > pMm_detail_sec_th)&
   	                 (iBestDetail < iSecDetail*pMm_detail_k_th/16);
   select_sec = pMm_mode==0? small_sad_diff :
   	            pMm_mode==1? (small_sad_diff&big_detail_diff):
			    (small_sad_diff | big_detail_diff);

   select_sec = pMm_med_en ==0? select_sec:select_sec&&( (abs_vec(pBestVec->vector[0],iMedVec[0])+
   	                                                    abs_vec(pBestVec->vector[1],iMedVec[1]))>
   	                                                    (abs_vec(pSecVec->vector[0],iMedVec[0])+
   	                                                     abs_vec(pSecVec->vector[1],iMedVec[1])) );   
   return select_sec;
}

UINT16 abs_vec(INT16 a, INT16 b)
   	{
     UINT16 c;
	 c = (a>b)?a-b:b-a;
	 return c;
    }



void me_3drs(const UINT8* pPreY, UINT16* pPreSize, T_FRM_PARITY iPrePolarity, 
             const UINT8* pCurY, UINT16* pCurSize, T_FRM_PARITY iCurPolarity,  
             UINT16 iYRowByte, UINT8 iYPxlByte, 
             const T_3DRS_PARAM* p3DRSParam, 
             INT16* pVector, int* pVecDims,
             UINT32* pSAD, int* pSADDims, UINT8* pAPL, UINT16* pDetail)
{
    INT16      iBlkIdx[2];
    UINT16     iBlkNum[2];
    UINT8      iCandIdx;
    UINT16     iPxlNumInBlk, iFetPxlNumInBlk;
    UINT8      iPreBlkData[MAX_BLK_SIZE_X*MAX_BLK_SIZE_Y*3];
    UINT8      iCurBlkData[MAX_BLK_SIZE_X*MAX_BLK_SIZE_Y*3];
    UINT8      iAPLBlkData[MAX_BLK_SIZE_X*MAX_BLK_SIZE_Y*3];
    UINT8      iBlkSize[2], iFetBlkSize[2];
    UINT16     iPicSize[2];
    INT16      iPos[2];
    UINT32     iSAD, iDistor,iDetail;
    UINT32     iPrevSAD;
    bool       bRewind;
	UINT8      iCurAPL;
	UINT16     iCurDetail,iBestDetail,iSecDetail;
	T_CAND_VEC ZERO_VEC;
    
    int        iBestIdx;
	int        iSecIdx;  //multimin henry
    int        iCandNum;
    T_CAND_VEC tCandVec[32];
    UINT16     iCurBlkIdx;
    
    UINT32     iMinDistor, iMinSAD;
    UINT32     iSecDistor, iSecSAD;//multimin
    const UINT8* pPredY;
    UINT16     iPredPicSize[2]; 
    int        iPredPolarity;
    bool       bMmFlag;   //multimin 0: bv, 1:sv
    INT16      iMedVec[2];
	UINT16     iFlashCnt;
	
    ZERO_VEC.vector[0] = 0;
	ZERO_VEC.vector[1] = 0;
	ZERO_VEC.penalty   = 0;
	ZERO_VEC.type      = 0;
	iFlashCnt = 0;
    if(p3DRSParam->temp_phase<32) //use prev
    {
        pPredY          = pPreY;
        iPredPicSize[0] = pPreSize[0];
        iPredPicSize[1] = pPreSize[1];
        iPredPolarity   = iPrePolarity;
    }
    else //use cur
    {
        pPredY          = pCurY;
        iPredPicSize[0] = pCurSize[0];
        iPredPicSize[1] = pCurSize[1];
        iPredPolarity = iCurPolarity;
    }

    iBlkSize[0]    = p3DRSParam->block_size[0];
    iBlkSize[1]    = p3DRSParam->block_size[1];
    iFetBlkSize[0] = iBlkSize[0]+2*p3DRSParam->overlap[0];
    iFetBlkSize[1] = iBlkSize[1]+2*p3DRSParam->overlap[1];
    iPicSize[0]    = pPreSize[0];
    iPicSize[1]    = (iPrePolarity==FRM_PARITY_PROGRESS) ? pPreSize[1] : pPreSize[1]*2;
    iBlkNum[0]     = (UINT16)floor(iPicSize[0]/iBlkSize[0]);
    iBlkNum[1]     = (UINT16)floor(iPicSize[1]/iBlkSize[1]);
    iCandNum       = get_cand_num(p3DRSParam);
    iPxlNumInBlk   = p3DRSParam->block_size[0] * p3DRSParam->block_size[1];
    iFetPxlNumInBlk = iFetBlkSize[0] * iFetBlkSize[1];

    for((iBlkIdx[1] = p3DRSParam->flipud_en? (iBlkNum[1]-1):0); 
        (p3DRSParam->flipud_en? (iBlkIdx[1]>=0):(iBlkIdx[1]<iBlkNum[1])); 
        (p3DRSParam->flipud_en?(iBlkIdx[1]--):(iBlkIdx[1]++)))
    {
        bRewind = (p3DRSParam->meander_en) && (iBlkIdx[1]%2 == 1);
        for((iBlkIdx[0] = bRewind? (iBlkNum[0]-1):0); 
            (bRewind ? (iBlkIdx[0]>= 0):(iBlkIdx[0]<= (iBlkNum[0]-1))); 
            (iBlkIdx[0] =bRewind?(iBlkIdx[0]-1):(iBlkIdx[0]+1))
	   )
        {
            iCurBlkIdx = iBlkIdx[0] + iBlkIdx[1]*iBlkNum[0];
			iPos[0] = iBlkIdx[0]*iBlkSize[0]; 
			iPos[1] = iBlkIdx[1]*iBlkSize[1];
			get_block(pPredY, iYRowByte, iYPxlByte, iPredPicSize, iPredPolarity, iPos,
				&ZERO_VEC, p3DRSParam->vec_precision, 0, //temp phase 0
				iBlkSize, true, iAPLBlkData);
			iCurAPL = calc_apl(iAPLBlkData, iBlkSize);
			iCurDetail = calc_detail(iAPLBlkData, iBlkSize);
			iFlashCnt += flashing_detect(p3DRSParam->prev_apl,iCurBlkIdx,iCurAPL,p3DRSParam->flh_th);
			//if((p3DRSParam->pred_mode==1) || (p3DRSParam->aplap_en==1))
            //{
			pred_vector_apl_adapt(iCurAPL, p3DRSParam, tCandVec, pVector, pAPL, iBlkIdx, bRewind,iMedVec);
            //}
            //else //if(p3DRSParam->pred_mode==0)
            //    pred_vector(p3DRSParam, tCandVec,pVector, iBlkIdx, bRewind);

            iPos[0] = iBlkIdx[0]*iBlkSize[0]-p3DRSParam->overlap[0]; 
            iPos[1] = iBlkIdx[1]*iBlkSize[1]-p3DRSParam->overlap[1];

            iMinDistor =64*512;
			iSecDistor =64*512;
			iBestIdx   = 0;
			iSecIdx    = 0;
            for(iCandIdx = 0; iCandIdx< iCandNum; iCandIdx++)
            {
                get_block(pPreY, iYRowByte, iYPxlByte,  
                          pPreSize, iPrePolarity,  iPos, 
                          &tCandVec[iCandIdx], p3DRSParam->vec_precision, p3DRSParam->temp_phase, 
                          iFetBlkSize, true, iPreBlkData);
		   			   
			
                get_block(pCurY, iYRowByte, iYPxlByte,
                          pCurSize, iCurPolarity, iPos, 
                          &tCandVec[iCandIdx], p3DRSParam->vec_precision, p3DRSParam->temp_phase, 
                          iFetBlkSize, false, iCurBlkData);
		   
                iSAD = get_sad(iPreBlkData, iCurBlkData, iFetPxlNumInBlk, p3DRSParam->y_only);
				iDetail = calc_detail(iPreBlkData,iFetBlkSize)/2 + calc_detail(iCurBlkData,iFetBlkSize)/2;
    			iDistor = iSAD + tCandVec[iCandIdx].penalty;
                if(iMinDistor > iDistor)
                {
                    iSecIdx  = iBestIdx;
					iSecDistor = iMinDistor;
					iSecSAD  = iMinSAD;
					iSecDetail = iBestDetail;
                    iBestIdx = iCandIdx;
                    iMinDistor = iDistor;
                    iMinSAD    = iSAD;
					iBestDetail= iDetail;
                }
            }
//multimin
 
           bMmFlag = multi_min(&tCandVec[iBestIdx],&tCandVec[iSecIdx],iMinSAD,iSecSAD,iBestDetail,iSecDetail, iMedVec, p3DRSParam->mm_sad_diff_th,
                              p3DRSParam->mm_detail_best_th,p3DRSParam->mm_detail_sec_th,p3DRSParam->mm_detail_k_th,p3DRSParam->mm_mode ,
                              p3DRSParam->mm_vector_cons_th0,p3DRSParam->mm_vector_cons_th1,p3DRSParam->mm_vector_cons_th2,p3DRSParam->mm_med_en);

           iBestIdx = p3DRSParam->mm_en? (bMmFlag? iSecIdx:iBestIdx):iBestIdx;
		   iMinDistor= p3DRSParam->mm_en? (bMmFlag? iSecDistor:iMinDistor):iMinDistor;
		   iMinSAD = p3DRSParam->mm_en? (bMmFlag? iSecSAD:iMinSAD):iMinSAD;
 
            if(p3DRSParam->same_temp_en)
            {
                iPrevSAD  = p3DRSParam->prev_sad[iCurBlkIdx];

                if((p3DRSParam->cur_high_priority) && ((iPrevSAD+p3DRSParam->same_temp_penalty)<iMinSAD) ||
                   (!p3DRSParam->cur_high_priority)&& (iPrevSAD < (iMinSAD+p3DRSParam->same_temp_penalty)))  //prev is best
                {
                    pVector[iCurBlkIdx]   = p3DRSParam->prev_vec[iCurBlkIdx];
                    pVector[iCurBlkIdx + iBlkNum[0]*iBlkNum[1]] 
                                          = p3DRSParam->prev_vec[iCurBlkIdx + iBlkNum[0]*iBlkNum[1]];
                                            pSAD[iCurBlkIdx] = iPrevSAD;
                }
                else
                {
                    pSAD[iCurBlkIdx] = iMinSAD;
                    pVector[iCurBlkIdx]   = tCandVec[iBestIdx].vector[0];
                    pVector[iCurBlkIdx + iBlkNum[0]*iBlkNum[1]] = tCandVec[iBestIdx].vector[1];
                }
                               
            }
            else{
                pSAD[iCurBlkIdx] = iMinSAD;
                pVector[iCurBlkIdx]   = tCandVec[iBestIdx].vector[0];
                pVector[iCurBlkIdx + iBlkNum[0]*iBlkNum[1]] = tCandVec[iBestIdx].vector[1];
            }
            pAPL[iCurBlkIdx]    = iCurAPL;
			pDetail[iCurBlkIdx] = iCurDetail;
        	//iFlashCnt += flashing_detect(pAPL,iCurBlkIdx,iCurAPL,p3DRSParam->flh_th);	
        }
    }
		#ifdef _DEBUG_FLASH_
	mexPrintf("Flashing count is: %d \n" ,iFlashCnt );
	    #endif
	FLD_ID ++;
}

void   parser_me_3drs_parameter(T_3DRS_PARAM* p3DRSParam, const mxArray* pmxaMEParam)
{
    char         sPredMode     [32] = "pred_mode";
    char         sPredSADFactor[32] = "pred_sad_factor";
    char         sSameTempEn   [32] = "same_temp_en";     //if 1, prev_vec has same phase with current, but another direction
    char         sCurHighPrio  [32] = "cur_high_priority";
    char         sSameTempPen  [32] = "same_temp_penalty";  
    char         sPrevSAD      [32] = "prev_sad";
    char         sPrevLRSAD    [32] = "prev_lr_sad";
	char         sPrevAPL      [32] = "prev_apl";
	char         sPrevLRAPL    [32] = "prev_lr_apl";
	char         sPrevDtl      [32] = "prev_dtl";
	char         sPrevLRDtl    [32] = "prev_lr_dtl";
    char         sFlipUDEn     [32] = "flipud_en";
    char         sCUCSadDiffThrK [32] = "cuc_sad_diff_thr_k";//cuc_thr = cuc_sad_diff_thr + cuc_sad_diff_thr_k * SADmin
    char         sCUCSadDiffThr  [32] = "cuc_sad_diff_thr";
    char         sCUCSadThr    [32] = "cuc_sad_thr";
    char         sBlkSz        [32] = "block_size";
    char         sBlkNum       [32] = "block_number";
    char         sPredEn       [32] = "pred_en";
    char         sUpdateEn     [32] = "update_en";
    char         sPredPos      [32] = "pred_pos";
    char         sPredPenalty  [32] = "pred_penalty";
    char         sUpdatePenalty[32] = "update_penalty";
    char         sUpdateLevel  [32] = "update_level";
    char         sZeroEn       [32] = "zero_en";
    char         sZeroPenalty  [32] = "zero_penalty";
    char         sAfmEn        [32] = "afm_en";
    char         sAfmParam     [32] = "afm_param";
    char         sAfmPenalty   [32] = "afm_penalty";
	char         sLRPredEn     [32] = "lrpred_en";	
	char         sLRPredPenalty[32] = "lrp_penalty";//	UINT16     lrp_penalty;
	char         sLRPPenKDtl   [32] = "lrp_k_dtl";
	char         sLRPPenKAPL   [32] = "lrp_k_apl";
    char         sPrevVec      [32] = "prev_vec";
	char         sPrevLRVec    [32] = "prev_lr_vec";
	char         sLRBlkRatio   [32] = "lr_block_ratio";
    char         sVecPrecision [32] = "vec_precision";
    char         sYOnly        [32] = "y_only";
    char         sTempPhase    [32] = "temp_phase";
    char         sMeander      [32] = "meander_en";
    char         sOverlap      [32] = "overlap";
	char         sFSEn         [32] = "fs_en";
	char         sFSPenalty    [32] = "fs_penalty";
	char         sFSStep       [32] = "fs_step";
	char         sAPLPEn          [32] = "aplap_en";
	char         sAPLPVDiffFactor [32] = "aplap_vdiff_factor";//UINT8
	char         sAPLPMMAPLDiffThr[32] = "aplap_mm_apl_diff_thr";//UINT8
	char         sAPLPMMVecDiffThr[32] = "aplap_mm_vec_diff_thr";//UINT16
    char         sMmSadDiffThr [32] = "mm_sad_diff_thr"; //UINT16
    char         sMmDetailBestThr [32] = "mm_detail_best_thr"; //UINT16
    char         sMmDetailSecThr  [32] = "mm_detail_sec_thr";  //UINT16
    char         sMmDetailkThr [32]= "mm_detail_k_thr" ;
    char         sMmMode [32] = "mm_mode";
    char         sMmVecConsThr0 [32] = "mm_vector_cons_thr0";
	char         sMmVecConsThr1 [32] = "mm_vector_cons_thr1";
	char         sMmVecConsThr2 [32] = "mm_vector_cons_thr2";
	char         sMmEn [32] = "mm_en";
	char         sMmMedEn [32] = "mm_med_en" ;
    char         sFlashThr [32] = "flash_thr";
	char         sCrossPhaseEn [32] = "crossphase_en";
	char         sCrossPhasePenalty [32] = "cp_penalty";
	
    int          iIdx;
    mxArray *    pmxaTemp;
    UINT8*       pui8Temp;
    INT8*        pi8Temp;
    UINT16*      pui16Temp;
    INT16*       pi16Temp;
    UINT32*      pui32Temp;

	
    mexPrintf("parser 3drs parameters\n");
    //get pred_mode
    pmxaTemp = mxGetField(pmxaMEParam, 0, sPredMode);
    pui8Temp = (UINT8*)  mxGetData(pmxaTemp);
    p3DRSParam->pred_mode = pui8Temp[0];
    mexPrintf("pred_mode: %d    ", p3DRSParam->pred_mode);
    //get pred_sad_factor
    pmxaTemp = mxGetField(pmxaMEParam, 0, sPredSADFactor);
    pui8Temp = (UINT8*)  mxGetData(pmxaTemp);
    p3DRSParam->pred_sad_factor = pui8Temp[0];
    mexPrintf("pred_sad_factor: %d    ", p3DRSParam->pred_sad_factor);
    //get same_temp_en
    pmxaTemp = mxGetField(pmxaMEParam, 0, sSameTempEn);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->same_temp_en  = (pui8Temp[0]!=0);
    mexPrintf("same_temp_en: %d\n", p3DRSParam->same_temp_en);    
    //get cur_high_priority
    pmxaTemp = mxGetField(pmxaMEParam, 0, sCurHighPrio);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->cur_high_priority  = (pui8Temp[0]!=0);
    mexPrintf("cur_high_priority: %d\n", p3DRSParam->cur_high_priority);   
    //get same_temp_penalty
    pmxaTemp = mxGetField(pmxaMEParam, 0, sSameTempPen);
    pui16Temp = (UINT16*)mxGetData(pmxaTemp);
    p3DRSParam->same_temp_penalty  =  pui16Temp[0];
    mexPrintf("same_temp_penalty %d\n", p3DRSParam->same_temp_penalty);
    //get prev_sad
    pmxaTemp = mxGetField(pmxaMEParam, 0, sPrevSAD);
    pui32Temp = (UINT32*)mxGetData(pmxaTemp);
    p3DRSParam->prev_sad  =  pui32Temp;
    mexPrintf("prev_sad @%X\n", p3DRSParam->prev_sad);
	//get prev_apl
    pmxaTemp = mxGetField(pmxaMEParam, 0, sPrevAPL);
    pui8Temp = (UINT8*)mxGetData(pmxaTemp);
    p3DRSParam->prev_apl  =  pui8Temp;
    mexPrintf("prev_apl @%X\n", p3DRSParam->prev_apl);
	//get prev_dtl
    pmxaTemp = mxGetField(pmxaMEParam, 0, sPrevDtl);
    pui16Temp = (UINT16*)mxGetData(pmxaTemp);
    p3DRSParam->prev_dtl  =  pui16Temp;
    mexPrintf("prev_dtl @%X\n", p3DRSParam->prev_dtl);
    //get prev_lr_sad
    pmxaTemp = mxGetField(pmxaMEParam, 0, sPrevLRSAD);
    pui32Temp = (UINT32*)mxGetData(pmxaTemp);
    p3DRSParam->prev_lr_sad  =  pui32Temp;
    mexPrintf("prev_lr_sad @%X\n", p3DRSParam->prev_lr_sad);
	//get prev_lr_apl
    pmxaTemp = mxGetField(pmxaMEParam, 0, sPrevLRAPL);
    pui8Temp = (UINT8*)mxGetData(pmxaTemp);
    p3DRSParam->prev_lr_apl  =  pui8Temp;
    mexPrintf("prev_lr_apl @%X\n", p3DRSParam->prev_lr_apl);
	//get prev_dtl
    pmxaTemp = mxGetField(pmxaMEParam, 0, sPrevLRDtl);
    pui16Temp = (UINT16*)mxGetData(pmxaTemp);
    p3DRSParam->prev_lr_dtl  =  pui16Temp;
    mexPrintf("prev_lr_dtl @%X\n", p3DRSParam->prev_lr_dtl);
	
    //get flipud_en
    pmxaTemp = mxGetField(pmxaMEParam, 0, sFlipUDEn);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->flipud_en  = (pui8Temp[0]!=0);
    mexPrintf("flipud_en: %d\n", p3DRSParam->flipud_en);    

    //get cuc_sad_diff_thr_k
    pmxaTemp = mxGetField(pmxaMEParam, 0, sCUCSadDiffThrK);
    pui8Temp = (UINT8*)  mxGetData(pmxaTemp);
    p3DRSParam->cuc_sad_diff_thr_k = pui8Temp[0];
    mexPrintf("cuc_sad_diff_thr_k: %d\n", p3DRSParam->cuc_sad_diff_thr_k);    

    //get cuc_sad_diff_thr
    pmxaTemp = mxGetField(pmxaMEParam, 0, sCUCSadDiffThr);
    pui16Temp = (UINT16*)  mxGetData(pmxaTemp);
    p3DRSParam->cuc_sad_diff_thr = pui16Temp[0];
    mexPrintf("cuc_sad_diff_thr: %d\n", p3DRSParam->cuc_sad_diff_thr);  

    //get cuc_sad_thr
    pmxaTemp = mxGetField(pmxaMEParam, 0, sCUCSadThr);
    pui32Temp = (UINT32*)  mxGetData(pmxaTemp);
    p3DRSParam->cuc_sad_thr = pui32Temp[0];
    mexPrintf("cuc_sad_thr: %d\n", p3DRSParam->cuc_sad_thr);  


    //get block size
    pmxaTemp = mxGetField(pmxaMEParam, 0, sBlkSz);
    pui8Temp = (UINT8*)mxGetData(pmxaTemp);
    p3DRSParam->block_size[0]  =  pui8Temp[0];
    p3DRSParam->block_size[1]  =  pui8Temp[1];
    mexPrintf("block_size %d,%d\n", p3DRSParam->block_size[0], p3DRSParam->block_size[1]);
    //get block number
    pmxaTemp = mxGetField(pmxaMEParam, 0, sBlkNum);
    pui16Temp = (UINT16*)mxGetData(pmxaTemp);
    p3DRSParam->block_number[0]  =  pui16Temp[0];
    p3DRSParam->block_number[1]  =  pui16Temp[1];
    mexPrintf("block_number %d,%d\n", p3DRSParam->block_number[0], p3DRSParam->block_number[1]);
    //get pred en
    pmxaTemp = mxGetField(pmxaMEParam, 0, sPredEn);
    pui8Temp = (UINT8*)  mxGetData(pmxaTemp);
    mexPrintf("PredEn: ");
    for(iIdx=0; iIdx<8; iIdx++){
        p3DRSParam->pred_en[iIdx]  =  (pui8Temp[iIdx]!=0);
        mexPrintf("%d  ", p3DRSParam->pred_en[iIdx]);
        }
    mexPrintf("\n");
    //get update en
    pmxaTemp = mxGetField(pmxaMEParam, 0, sUpdateEn);
    pui8Temp = (UINT8*)  mxGetData(pmxaTemp);
    mexPrintf("UpdateEn: ");
    for(iIdx=0; iIdx<8; iIdx++){
        p3DRSParam->update_en[iIdx]  =  (pui8Temp[iIdx]!=0);
        mexPrintf("%d  ", p3DRSParam->update_en[iIdx]);
        }
    mexPrintf("\n");
    //get update level
    pmxaTemp = mxGetField(pmxaMEParam, 0, sUpdateLevel);
    pui8Temp = (UINT8*)mxGetData(pmxaTemp);
    p3DRSParam->update_level[0]  =  pui8Temp[0];
    p3DRSParam->update_level[1]  =  pui8Temp[1];
    mexPrintf("update_level %d,%d\n", p3DRSParam->update_level[0], p3DRSParam->update_level[1]);
    //get pred pos
    pmxaTemp = mxGetField(pmxaMEParam, 0, sPredPos);
    pi8Temp  = (INT8*)  mxGetData(pmxaTemp);
    mexPrintf("PredPos: ");
    for(iIdx=0; iIdx<8; iIdx++)
    {
        p3DRSParam->pred_pos[iIdx][0]  =  pi8Temp[2*iIdx];
        p3DRSParam->pred_pos[iIdx][1]  =  pi8Temp[2*iIdx+1];
        mexPrintf("(%d, %d)",p3DRSParam->pred_pos[iIdx][0], p3DRSParam->pred_pos[iIdx][1]); 
    }
    mexPrintf("\n");
    //get pred penalty
    mexPrintf("PredPen: ");
    pmxaTemp = mxGetField(pmxaMEParam, 0, sPredPenalty);
    pui16Temp = (UINT16*) mxGetData(pmxaTemp);
    for(iIdx=0; iIdx<8; iIdx++)
    {
        p3DRSParam->pred_penalty[iIdx] = pui16Temp[iIdx];
        mexPrintf("%d  ", p3DRSParam->pred_penalty[iIdx]);
    }
    mexPrintf("\n");
    //get update penalty
    pmxaTemp = mxGetField(pmxaMEParam, 0, sUpdatePenalty);
    pui16Temp = (UINT16*) mxGetData(pmxaTemp);
    p3DRSParam->update_penalty  = pui16Temp[0];
    mexPrintf("update_penalty: %d\n", p3DRSParam->update_penalty); 
    //get zero en
    pmxaTemp = mxGetField(pmxaMEParam, 0, sZeroEn);
    pui8Temp = (UINT8*)  mxGetData(pmxaTemp);
    p3DRSParam->zero_en = (pui8Temp[0]!=0);
    mexPrintf("zero_en: %d    ", p3DRSParam->zero_en);
    //get zero penalty
    pmxaTemp = mxGetField(pmxaMEParam, 0, sZeroPenalty);
    pui16Temp = (UINT16*) mxGetData(pmxaTemp);
    p3DRSParam->zero_penalty  = pui16Temp[0];
    mexPrintf("zero_penalty: %d\n", p3DRSParam->zero_penalty);    
    //get afm en
    pmxaTemp = mxGetField(pmxaMEParam, 0, sAfmEn);
    pui8Temp = (UINT8*)  mxGetData(pmxaTemp);
    p3DRSParam->afm_en = (pui8Temp[0]!=0);
    mexPrintf("afm_en: %d\n", p3DRSParam->afm_en);
    //get afm param
    pmxaTemp = mxGetField(pmxaMEParam, 0, sAfmParam);
    pi16Temp = (INT16*) mxGetData(pmxaTemp);
    mexPrintf("afm_param:");
    for(iIdx=0; iIdx<8; iIdx++)
    {
        p3DRSParam->afm_param[iIdx] = pi16Temp[iIdx];
        mexPrintf("%d  ", p3DRSParam->afm_param[iIdx]);
    }
    mexPrintf("\n");
    //get afm penalty
    pmxaTemp = mxGetField(pmxaMEParam, 0, sAfmPenalty);
    pui16Temp = (UINT16*) mxGetData(pmxaTemp);
    p3DRSParam->afm_penalty  = pui16Temp[0];
    mexPrintf("afm_penalty: %d\n", p3DRSParam->afm_penalty); 
    //get prev_vec
    pmxaTemp = mxGetField(pmxaMEParam, 0, sPrevVec);
    pi16Temp = (INT16*) mxGetData(pmxaTemp);
    p3DRSParam->prev_vec  = pi16Temp;
    mexPrintf("prev_vector: @%d\n", p3DRSParam->prev_vec); 
    //get vec precision
    pmxaTemp = mxGetField(pmxaMEParam, 0, sVecPrecision);
    pui8Temp = (UINT8*)mxGetData(pmxaTemp);
    p3DRSParam->vec_precision[0]  =  1<< pui8Temp[0];
    p3DRSParam->vec_precision[1]  =  1<< pui8Temp[1];
    mexPrintf("vec_precision %d,%d\n", p3DRSParam->vec_precision[0], p3DRSParam->vec_precision[1]);
    //get y_only
    pmxaTemp = mxGetField(pmxaMEParam, 0, sYOnly);
    pui8Temp = (UINT8*)  mxGetData(pmxaTemp);
    p3DRSParam->y_only = (pui8Temp[0]!=0);
    mexPrintf("y_only: %d    ", p3DRSParam->y_only);
    //get temp_phase
    pmxaTemp = mxGetField(pmxaMEParam, 0, sTempPhase);
    pui8Temp = (UINT8*)  mxGetData(pmxaTemp);
    p3DRSParam->temp_phase = pui8Temp[0];
    mexPrintf("temp_phase: %d    ", p3DRSParam->temp_phase);
    //get meander
    pmxaTemp = mxGetField(pmxaMEParam, 0, sMeander);
    pui8Temp = (UINT8*)  mxGetData(pmxaTemp);
    p3DRSParam->meander_en = pui8Temp[0];
    //get overlap
    pmxaTemp = mxGetField(pmxaMEParam, 0, sVecPrecision);
    pui8Temp = (UINT8*)mxGetData(pmxaTemp);
    p3DRSParam->overlap[0]  =  pui8Temp[0];
    p3DRSParam->overlap[1]  =  pui8Temp[1];
    mexPrintf("overlap %d,%d\n", p3DRSParam->overlap[0], p3DRSParam->overlap[1]);
	//get fs_en
    pmxaTemp = mxGetField(pmxaMEParam, 0, sFSEn);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->fs_en  = (pui8Temp[0]!=0);
    mexPrintf("fs_en: %d\n", p3DRSParam->fs_en);
	//get fs_penalty
    pmxaTemp = mxGetField(pmxaMEParam, 0, sFSPenalty);
    pui16Temp = (UINT16*) mxGetData(pmxaTemp);
    p3DRSParam->fs_penalty  = pui16Temp[0];
    mexPrintf("fs_penalty: %d\n", p3DRSParam->fs_penalty);    
	//get fs_range
    pmxaTemp = mxGetField(pmxaMEParam, 0, sFSStep);
    pui16Temp = (UINT16*) mxGetData(pmxaTemp);
	for(iIdx=0; iIdx<8; iIdx++)
    {
        p3DRSParam->fs_step[iIdx] = pui16Temp[iIdx];
        mexPrintf("%d  ", p3DRSParam->fs_step[iIdx]);
    }
	//	char         sAPLPEn          [32] = "aplap_en";
    pmxaTemp = mxGetField(pmxaMEParam, 0, sAPLPEn);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->aplap_en  = (pui8Temp[0]!=0);
    mexPrintf("aplap_en: %d\n", p3DRSParam->aplap_en);
    //char         sAPLPVDiffFactor[32] = "aplap_vdiff_factor";//UINT8
    pmxaTemp = mxGetField(pmxaMEParam, 0, sAPLPVDiffFactor);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->aplap_vdiff_factor  = pui8Temp[0];
    mexPrintf("aplap_vdiff_factor: %d\n", p3DRSParam->aplap_vdiff_factor);
	//char         sAPLPMMAPLDiffThr[32] = "aplap_mm_apl_diff_thr";//UINT8
    pmxaTemp = mxGetField(pmxaMEParam, 0, sAPLPMMAPLDiffThr);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->aplap_mm_apl_diff_thr  = pui8Temp[0];
    mexPrintf("aplap_mm_apl_diff_thr: %d\n", p3DRSParam->aplap_mm_apl_diff_thr);
	//char         sAPLPMMVecDiffThr[32] = "aplap_mm_vec_diff_thr";//INT16
	pmxaTemp = mxGetField(pmxaMEParam, 0, sAPLPMMVecDiffThr);
    pui16Temp = (UINT16*) mxGetData(pmxaTemp);
    p3DRSParam->aplap_mm_vec_diff_thr  = pui16Temp[0];
    mexPrintf("aplap_mm_vec_diff_thr: @%d\n", p3DRSParam->aplap_mm_vec_diff_thr); 
	//bool       lrpred_en;    //pen = lrp_k_dtl * dtl + lrp_k_apl * apl_diff
    pmxaTemp = mxGetField(pmxaMEParam, 0, sLRPredEn);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->lrpred_en  = (pui8Temp[0]!=0);
    mexPrintf("lrpred_en: %d\n", p3DRSParam->lrpred_en);
	//UINT8      lrp_k_dtl;
    pmxaTemp = mxGetField(pmxaMEParam, 0, sLRPPenKDtl);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->lrp_k_dtl  = pui8Temp[0];
    mexPrintf("lrp_k_dtl: %d\n", p3DRSParam->lrp_k_dtl);
	//UINT8      lrp_k_apl;  
    pmxaTemp = mxGetField(pmxaMEParam, 0, sLRPPenKAPL);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->lrp_k_apl  = pui8Temp[0];
    mexPrintf("lrp_k_dtl: %d\n", p3DRSParam->lrp_k_apl);
	//get sLRPredPenalty
    pmxaTemp = mxGetField(pmxaMEParam, 0, sFSPenalty);
    pui16Temp = (UINT16*) mxGetData(pmxaTemp);
    p3DRSParam->lrp_penalty  = pui16Temp[0];
    mexPrintf("lrp_penalty: %d\n", p3DRSParam->lrp_penalty);    
	//INT16*     prev_lr_vec;
    pmxaTemp = mxGetField(pmxaMEParam, 0, sPrevLRVec);
    pi16Temp = (INT16*) mxGetData(pmxaTemp);
    p3DRSParam->prev_lr_vec  = pi16Temp;
    mexPrintf("prev_lr_vec: @%d\n", p3DRSParam->prev_lr_vec); 
	//UINT8      lr_block_ratio;  
    pmxaTemp = mxGetField(pmxaMEParam, 0, sLRBlkRatio);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->lr_block_ratio[0]  = pui8Temp[0];
    p3DRSParam->lr_block_ratio[1]  = pui8Temp[1];
    mexPrintf("lr_block_ratio: %d  %d\n", p3DRSParam->lr_block_ratio[0], 
		                                  p3DRSParam->lr_block_ratio[1]);

	//UINT16*     mm_sad_diff_th;
    pmxaTemp = mxGetField(pmxaMEParam, 0, sMmSadDiffThr);
    pui16Temp = (UINT16*) mxGetData(pmxaTemp);
    p3DRSParam->mm_sad_diff_th  = pui16Temp[0];
	mexPrintf("mm_sad_diff_th: @%d\n", p3DRSParam->mm_sad_diff_th);
	//UINT16*     mm_detail_best_th;
    pmxaTemp = mxGetField(pmxaMEParam, 0, sMmDetailBestThr);
    pui16Temp = (UINT16*) mxGetData(pmxaTemp);
    p3DRSParam->mm_detail_best_th= pui16Temp[0];
	mexPrintf("mm_detail_best_th: @%d\n", p3DRSParam->mm_detail_best_th);
	//UINT16*     mm_detail_sec_th;
    pmxaTemp = mxGetField(pmxaMEParam, 0, sMmDetailSecThr);
    pui16Temp = (UINT16*) mxGetData(pmxaTemp);
    p3DRSParam->mm_detail_sec_th= pui16Temp[0];
	mexPrintf("mm_detail_sec_th: @%d\n", p3DRSParam->mm_detail_sec_th);
	//UINT8      mm_detail_k_th;
    pmxaTemp = mxGetField(pmxaMEParam, 0, sMmDetailkThr);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->mm_detail_k_th= pui8Temp[0];
	mexPrintf("mm_detail_k_th: @%d\n", p3DRSParam->mm_detail_k_th);
	//UINT8      mm_mode;
    pmxaTemp = mxGetField(pmxaMEParam, 0, sMmMode);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->mm_mode= pui8Temp[0];
	mexPrintf("mm_mode: @%d\n", p3DRSParam->mm_mode);
	//UINT8      mm_vec_cons_thr0;
    pmxaTemp = mxGetField(pmxaMEParam, 0, sMmVecConsThr0);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->mm_vector_cons_th0= pui8Temp[0];
	mexPrintf("mm_vector_cons_th0: @%d\n", p3DRSParam->mm_vector_cons_th0);
	//UINT8      mm_vec_cons_thr1;
    pmxaTemp = mxGetField(pmxaMEParam, 0, sMmVecConsThr1);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->mm_vector_cons_th1= pui8Temp[0];
	mexPrintf("mm_vector_cons_th1: @%d\n", p3DRSParam->mm_vector_cons_th1);
	//UINT8      mm_vec_cons_thr2;
    pmxaTemp = mxGetField(pmxaMEParam, 0, sMmVecConsThr2);
    pui8Temp = (UINT8*) mxGetData(pmxaTemp);
    p3DRSParam->mm_vector_cons_th2= pui8Temp[0];
	mexPrintf("mm_vector_cons_th2: @%d\n", p3DRSParam->mm_vector_cons_th2);
	//get mm en
    pmxaTemp = mxGetField(pmxaMEParam, 0, sMmEn);
    pui8Temp = (UINT8*)  mxGetData(pmxaTemp);
    p3DRSParam->mm_en = (pui8Temp[0]!=0);
    mexPrintf("mm_en: %d    ", p3DRSParam->mm_en);
	//get mm med en
    pmxaTemp = mxGetField(pmxaMEParam, 0, sMmMedEn);
    pui8Temp = (UINT8*)  mxGetData(pmxaTemp);
    p3DRSParam->mm_med_en = (pui8Temp[0]!=0);
    mexPrintf("mm_med_en: %d    ", p3DRSParam->mm_med_en);
	//UINT16*     flash_thr;
    pmxaTemp = mxGetField(pmxaMEParam, 0, sFlashThr);
    pui16Temp = (UINT16*) mxGetData(pmxaTemp);
    p3DRSParam->flh_th= pui16Temp[0];
	mexPrintf("flash_thr: @%d\n", p3DRSParam->flh_th);

	//char         sCrossPhase [32] = "crossphase_en"; 
	pmxaTemp = mxGetField(pmxaMEParam, 0, sCrossPhaseEn);
	pui8Temp = (UINT8*)  mxGetData(pmxaTemp);
	p3DRSParam->crossphase_en= (pui8Temp[0]!=0);
	mexPrintf("CrossPhaseEn: %d	", p3DRSParam->crossphase_en);

	//char         sCrossPhasePenalty [32] = "cp_penalty";
	pmxaTemp = mxGetField(pmxaMEParam, 0, sCrossPhasePenalty);
    pui16Temp = (UINT16*) mxGetData(pmxaTemp);
    p3DRSParam->cp_penalty = pui16Temp[0];
    mexPrintf("cp_penalty: %d\n", p3DRSParam->cp_penalty); 

	
	return;
    

}


void  unify_fb_parameters(UINT8 iFormat, 
                          const UINT8 *pYUV, const UINT16 *pSize,
                          UINT8 **pY, UINT16 *pYRowByte, UINT8 *pYPxlByte,
                          UINT8 **pU, UINT16 *pURowByte, UINT8 *pUPxlByte,
                          UINT8 **pV, UINT16 *pVRowByte, UINT8 *pVPxlByte)
{
    switch(iFormat)
	{
    case 0:  //planer, 444, Yplaner, Uplaner, Vplaner
        pY[0]         = (UINT8*) pYUV;
        pYRowByte[0]  = pSize[0];
        pYPxlByte[0]  = 1;
        pU[0]         = (UINT8*)(pYUV + pSize[0]*pSize[1]);
		pURowByte[0]  = pSize[0];
		pUPxlByte[0]  = 1; 
        pV[0]        = (UINT8*)(pU + pSize[0]*pSize[1]);
		pVRowByte[0]  = pSize[0];
		pVPxlByte[0]  = 1; 
	    break;
	default: 
        mexErrMsgTxt("the FB format is still not supported.");//
		break;
	}
    return;
}

void  parser_fb_parameters(const mxArray* pmxaPreYUV, const mxArray* pmxaPrePolarity,
                           const mxArray* pmxaCurYUV, const mxArray* pmxaCurPolarity,
                           const mxArray* pmxaFormat,
                           UINT8 **pPreYUV, T_FRM_PARITY* pPrePolarity, UINT16* pPreSize,
                           UINT8 **pCurYUV, T_FRM_PARITY* pCurPolarity, UINT16* pCurSize,
                           UINT8 *pFormat)
{
	const unsigned int *            pTmpI;
	//picture data
    pCurYUV[0]       =    (UINT8*)  mxGetData(pmxaCurYUV);
    pTmpI            =              mxGetDimensions(pmxaCurYUV);
    pCurSize[0]      =    (UINT16)  pTmpI[0];
    pCurSize[1]      =    (UINT16)  pTmpI[1];
	pCurPolarity[0]  =    (T_FRM_PARITY) (*(UINT8*)  mxGetData(pmxaCurPolarity));

    pPreYUV[0]       =    (UINT8*)  mxGetData(pmxaPreYUV);
    pTmpI            =              mxGetDimensions(pmxaPreYUV);
    pPreSize[0]      =    (UINT16)  pTmpI[0];
    pPreSize[1]      =    (UINT16)  pTmpI[1];
	pPrePolarity[0]  =    (T_FRM_PARITY) (*(UINT8*)  mxGetData(pmxaPrePolarity));

	pFormat[0]       =   *(UINT8*) mxGetData(pmxaFormat);

    return;
}

mxArray *  call_3drs_me( 
		         const mxArray* pmxaPreYUV, const mxArray* pmxaPrePolarity,
		         const mxArray* pmxaCurYUV, const mxArray* pmxaCurPolarity,
                         const mxArray* pmxaFormat,
                         const mxArray* pmxaMEParam)
{
    //input parameters
    UINT8        *pCurYUV, *pPreYUV;
    UINT8        *pCurY, *pCurU, *pCurV;
    UINT8        *pPreY, *pPreU, *pPreV;
    UINT16       iYRowByte, iURowByte, iVRowByte;
    UINT8        iYPxlByte, iUPxlByte, iVPxlByte; 
    UINT8        iFormat;
    T_FRM_PARITY iCurPolarity, iPrePolarity;
    UINT16       iCurSize[2], iPreSize[2];
    
    //output parameters
    mxArray*     pResult;
    char**       psFldNames;
    const int    nFldNum      = 5;
    char         sVector [32] = "vector";
    char         sSAD    [32] = "SAD";
    char         sBlkSz  [32] = "block_size";
    char         sAPL    [32] = "APL"; 
    char         sDetail [32] = "detail"; 
    
    mxArray*     pmxaBlkSize;
    double*      pBlkSize;
    mxArray*     pmxaVector;
    INT16*       pVector;
    int          pVecDims[3];
    mxArray*     pmxaSAD;
    UINT32*      pSAD;
    int          pSADDims[2];
    mxArray*     pmxaAPL;
    UINT8*       pAPL;
    mxArray*     pmxaDetail;
    UINT16*      pDetail;
    
    T_3DRS_PARAM t3DRSParam;
    
    //get frame buffer
    pCurYUV = pCurY = pCurU = pCurV = NULL;
    pPreYUV = pPreY = pPreU = pPreV = NULL;
    parser_fb_parameters(pmxaPreYUV, pmxaPrePolarity, 
                         pmxaCurYUV, pmxaCurPolarity,
                         pmxaFormat,
                         &pPreYUV, &iPrePolarity, iPreSize,
                         &pCurYUV, &iCurPolarity, iCurSize,
                         &iFormat);

    if(!verify_common_parameters(iCurSize, iCurPolarity,
                                 iPreSize, iPrePolarity))
    {
        mexErrMsgTxt("me need 6 input parameters."); 
    }
    unify_fb_parameters(iFormat, pPreYUV, iPreSize,
                        &pPreY, &iYRowByte, &iYPxlByte,
                        &pPreU, &iURowByte, &iUPxlByte,
                        &pPreV, &iVRowByte, &iVPxlByte);

    unify_fb_parameters(iFormat, pCurYUV, iCurSize,
                        &pCurY, &iYRowByte, &iYPxlByte,
                        &pCurU, &iURowByte, &iUPxlByte,
                        &pCurV, &iVRowByte, &iVPxlByte);

    //get 3DRS parameter
    parser_me_3drs_parameter(&t3DRSParam, pmxaMEParam);
    
    //create output structure
    psFldNames = mxCalloc(nFldNum, sizeof(char*));
    psFldNames[0] = sBlkSz;
    psFldNames[1] = sVector;
    psFldNames[2] = sSAD;
    psFldNames[3] = sAPL;
    psFldNames[4] = sDetail;
    pResult    = mxCreateStructMatrix(1, 1, nFldNum, psFldNames);
    mexPrintf("create struct success\n");
    mxFree(psFldNames);

    pmxaBlkSize  =  mxCreateDoubleMatrix( 1, 2, mxREAL);
    pBlkSize     =  mxGetData(pmxaBlkSize);
    pBlkSize[0]  =  t3DRSParam.block_size[0];
    pBlkSize[1]  =  t3DRSParam.block_size[1];
    
    pVecDims[2]  =  2;
    pVecDims[0]  =  (int)ceil(iCurSize[0]/pBlkSize[0]);
    pVecDims[1]  =  (int)ceil(iCurSize[1]/pBlkSize[1]);
    mexPrintf("PictureSize: %d, %d\n", iCurSize[0], iCurSize[1]);
    mexPrintf("VectorDims: %d, %d\n", pVecDims[0], pVecDims[1]);
    pmxaVector   =  mxCreateNumericArray(3, (const mwSize *)pVecDims, mxINT16_CLASS, mxREAL);
    pVector      =  (INT16*)mxGetData(pmxaVector);
    
    pSADDims[0]  =  pVecDims[0];
    pSADDims[1]  =  pVecDims[1];
    pmxaSAD      =  mxCreateNumericArray(2, (const mwSize *)pSADDims, mxUINT32_CLASS, mxREAL);
    pSAD         =  (UINT32*)mxGetData(pmxaSAD);

    pmxaAPL      =  mxCreateNumericArray(2, (const mwSize *)pSADDims, mxUINT8_CLASS, mxREAL);
    pAPL         =  (UINT8*)mxGetData(pmxaAPL);

    pmxaDetail   =  mxCreateNumericArray(2, (const mwSize *)pSADDims, mxUINT16_CLASS, mxREAL);
    pDetail      =  (UINT16*)mxGetData(pmxaDetail);
    //call 3drs 
    me_3drs(pPreY, iPreSize, iPrePolarity, 
            pCurY, iCurSize, iCurPolarity,  
            iYRowByte, iYPxlByte, 
            &t3DRSParam, 
            pVector, pVecDims,
            pSAD, pSADDims, pAPL, pDetail);
   
    
    //fill result struct
    mxSetFieldByNumber(pResult, 0, 0, pmxaBlkSize);
    mxSetFieldByNumber(pResult, 0, 1, pmxaVector);
    mxSetFieldByNumber(pResult, 0, 2, pmxaSAD);
    mxSetFieldByNumber(pResult, 0, 3, pmxaAPL);
    mxSetFieldByNumber(pResult, 0, 4, pmxaDetail);
    
    return pResult;
}

/* mex file interface */
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray* prhs[] )
{


    const mxArray      *pmxaMEParam, *pmxaPreYUV, *pmxaPrePolarity, *pmxaCurYUV, *pmxaCurPolarity, *pmxaFormat;
	

    if (nrhs != 6) { 
        mexErrMsgTxt("me need 6 input parameters."); 
        return;
    }
    else if (nlhs != 1) {
        mexErrMsgTxt("me need 1 output arguments."); 
        return;
    } 
	
    pmxaPreYUV       =  PREVIOUS_Y;
    pmxaPrePolarity  =  PRE_POLARITY;
    pmxaCurYUV       =  CURRENT_Y;
    pmxaCurPolarity  =  CUR_POLARITY;
    pmxaFormat       =  FRAME_FORMAT;
    pmxaMEParam      =  ME_PARAMETER;
    mexPrintf("\n\ncall me\n");
    ME_RESULT = call_3drs_me(
                             pmxaPreYUV, pmxaPrePolarity, 
                             pmxaCurYUV, pmxaCurPolarity, 
                             pmxaFormat, pmxaMEParam);

    return;
}

